import open3d as o3d
import numpy as np
import matplotlib.pyplot as plt
import math
import json

# 初始化默认
# CAMERA_LENGTH = 12.5 # 线扫单位长度（微米）
# CAMERA_WIDTH = 12.5  # 线扫单位宽度（微米）
calculate_cfg = "utils/detection_3d/calculate_cfg.json"
# calculate_cfg = "./calculate_cfg.json"
with open(calculate_cfg, 'r') as f:
    config = json.load(f)
    CAMERA_LENGTH = float(config['x_step_up'])
    CAMERA_WIDTH = float(config['y_step_up'])

CAMERA_HEIGHT = 1  # 线扫单位高度 （毫米）
V_RADIO = 1000 * 1000 * 1  # 转化为立方毫米的比例
CONSTANT_UP = CAMERA_WIDTH * CAMERA_HEIGHT * CAMERA_LENGTH / V_RADIO  # 求和之后乘的系数

VIS_RADIO = 80  # x，y坐标的和z坐标统一单位的系数


def denoising_by_remove_points(x, y, z, nb_neighbors=40, std_ratio=1.0, show=False):
    """
    半径统计离散点去除
    :param x:
    :param y:
    :param z:
    :param nb_neighbors:
    :param std_ratio:
    :param show:
    :return:
    """
    area_pcd = numpy2pcd(x, y, z)
    cl, ind = area_pcd.remove_statistical_outlier(nb_neighbors=nb_neighbors,
                                                  std_ratio=std_ratio)
    if show:
        from utils.detection_3d.visualization import display_inlier_outlier
        display_inlier_outlier(area_pcd, ind)
    area_pcd = area_pcd.select_by_index(ind)
    x_points, y_points, z_points = pcd2numpy(area_pcd)
    return x_points, y_points, z_points


def numpy2pcd(x, y, z):
    """
    numpy 数据转为open3d 的pcd点云
    :param x: numpy x轴数据
    :param y: numpy y轴数据
    :param z: numpy z轴数据
    :return:  o3d.geometry.PointCloud
    """
    pcd = o3d.geometry.PointCloud()
    m3d = np.array([x, y, z]).T
    pcd.points = o3d.utility.Vector3dVector(m3d)
    return pcd


def points2pcd(xyz_points):
    """
    元组拆分 生成open3d 的pcd点云
    :param xyz_points: tuple (x,y,z)
    :return: o3d.geometry.PointCloud
    """
    x, y, z = xyz_points
    pcd = o3d.geometry.PointCloud()
    m3d = np.array([x, y, z]).T
    pcd.points = o3d.utility.Vector3dVector(m3d)
    return pcd


def pcd2numpy(pcd):
    """
    点云数据转化为numpy
    :param pcd: o3d.geometry.PointCloud
    :return: x_points-> np.array, y_points-> np.array, z_points-> np.array
    """
    xyz_np = np.asarray(pcd.points)
    x_points = xyz_np[:, 0]
    y_points = xyz_np[:, 1]
    z_points = xyz_np[:, 2]
    return x_points, y_points, z_points


def parse_npy(npy_file):
    """
    显示npy格式的三维点云数据
    :param npy_file: npy文件路径
    :return: (x_points->np.array, y_points, z_points) -> tuple
    """

    z_points = np.load(npy_file)
    y_num, x_num = z_points.shape

    unit_x_point = np.arange(x_num)
    x_points = np.tile(unit_x_point, y_num) / VIS_RADIO

    unit_y_point = np.arange(y_num)
    y_points = np.repeat(unit_y_point, x_num) / VIS_RADIO

    z_points = z_points.reshape((-1))

    nan_index = np.isnan(z_points)

    # remove nan points
    x_points = x_points[~nan_index]
    y_points = y_points[~nan_index]
    z_points = z_points[~nan_index]

    return (x_points, y_points, z_points)


def data_filter_by_area(x_points, y_points, z_points, x_min=None, x_max=None, y_min=None, y_max=None):
    """
    通过x_min, x_max, y_min, y_max过滤数据
    :param x_points:
    :param y_points:
    :param z_points:
    :param x_min:
    :param x_max:
    :param y_min:
    :param y_max:
    :return:
    """
    print(x_min)
    print(x_max)
    print(y_min)
    print(y_max)
    if x_min is None:
        x_min = x_points.min()
    if x_max is None:
        x_max = x_points.max()
    if y_min is None:
        y_min = y_points.min()
    if y_max is None:
        y_max = y_points.max()

    index_0 = x_points <= x_max
    index_1 = x_points >= x_min
    index_2 = y_points <= y_max
    index_3 = y_points >= y_min
    _01 = np.logical_and(index_0, index_1)
    _012 = np.logical_and(_01, index_2)
    index = np.logical_and(_012, index_3)
    return x_points[index], y_points[index], z_points[index]


def data_filter_by_Z_value(x, y, z, Z_BOTTOM, compare=1):
    """
    通过z坐标值过滤数据
    :param x:
    :param y:
    :param z:
    :param Z_BOTTOM: 底面z坐标
    :param compare:
        -1 取小于Z_BOTTOM 的坐标
        1  取大于Z_BOTTOM 的坐标
    :return:
    """
    if compare == 1:
        aim_index = z > Z_BOTTOM
    else:
        aim_index = z < Z_BOTTOM
    x_points = x[aim_index]
    y_points = y[aim_index]
    z_points = z[aim_index] - Z_BOTTOM
    # z_points = z[aim_index]
    return x_points, y_points, z_points


def draw_z_points(x, z):
    plt.scatter(x, z)
    plt.show()


def add_bottom(x, y, z, bottom_value):
    """
    增加底面
    :return:
    """
    x = np.tile(x, 2).reshape((-1))
    y = np.tile(y, 2).reshape((-1))
    z_bottom = np.full_like(z, bottom_value)
    z = np.array([z, z_bottom]).reshape(-1)
    return x, y, z


# ===================正面计算的工具函数===============================

def static_center_value(array, radio=2, thr=1.2):
    """
    去除太高 太低的点 然后取平均值
    :param array: 一维np数组 -> np.array (n,)
    :param radio: 去除的点数占总点数的比例 1/1.5
    :return:
    """

    # 去除相差太远的点
    a_list = array.tolist()
    iter = int(len(a_list) / radio)
    # 循环删除
    for i in range(iter):
        if sum(a_list) / len(a_list) - min(a_list) < thr:
            a_list.remove(min(a_list))
        if max(a_list) - sum(a_list) / len(a_list) > thr:
            a_list.remove(max(a_list))

    return sum(a_list) / len(a_list)


def get_bottom_value(x, y, z, remove_points=False, iter=5, SUB=0.0125):
    """
    通过边缘的直线，粗略获取底面pcd板子的z坐标
    :param x:
    :param y:
    :param z:
    :param iter: 迭代次数
    :param SUB:
    :return:
    """

    if remove_points:
        # 过滤离散点 进行去噪和下采样
        x, y, z = denoising_by_remove_points(x, y, z, nb_neighbors=40, std_ratio=0.05, show=False)

    # 最下面的直线区域获取底面：获取最小的iter个y值 获取在这个y范围内的z的统计值
    y1 = y.min()
    y2 = y.min() + iter * SUB
    index_10 = y > y1
    index_11 = y < y2
    index1 = np.logical_and(index_11, index_10)
    bottom_down = z[index1].mean()

    # 最上面的直线区域获取底面： 获取最大的iter个y值 获取在这个y范围内的z的统计值
    y3 = y.max()
    y4 = y.max() - iter * SUB
    index_20 = y > y4
    index_21 = y < y3
    index2 = np.logical_and(index_21, index_20)
    bottom_up = z[index2].mean()

    # 最左边的区域获取底面
    x1 = x.max() / 40 + 2
    x2 = x1 + iter * SUB * 2
    index_30 = x > x1
    index_31 = x < x2
    index3 = np.logical_and(index_30, index_31)
    bottom_left = z[index3].mean()

    return bottom_down, bottom_up, bottom_left


def get_real_bottom(bottom_value, z, thr=1.8, step=0.0125):
    """
    根据粗定位的底面高度 获取真实高度
    找某一高度的点数，点数最多的高度就是真实高度
    :param bottom_value:
    :param z:
    :param thr: 偏移量的阈值
    :param step: 梯度步长
    :return:
    """

    b_max = bottom_value + thr
    b_min = bottom_value - thr
    bottom_list = np.arange(b_min, b_max, step)
    bottom_sub = []  # 进行前后差值统计的列表，寻找跳变点
    for i in range(len(bottom_list) - 1):
        z_pre = bottom_list[i]
        z_last = bottom_list[i + 1]
        z_pre_len = np.sum(z >= z_pre)
        z_last_len = np.sum(z >= z_last)
        # print(z_pre,z_pre_len-z_last_len)
        bottom_sub.append(z_pre_len - z_last_len)
    jump_index = bottom_sub.index(max(bottom_sub))
    # real_bottom_jump = bottom_list[jump_index]
    threshold_list = np.array(bottom_sub) > 800
    threshold_index = jump_index
    for i in range(jump_index, len(threshold_list)):
        if threshold_list[i]:
            threshold_index = i
    return bottom_list[threshold_index]


# ===================正面计算的工具函数===============================


## ===================测面计算的工具函数==============================


# ===================================未使用=============================

def get_real_dy(R, ori_dy=12.5):
    """
    由于旋转扫描，Y的间距12.5是弧长，实际上只有一个弦长
    :param R: 点到旋转圆心的距离
    :param ori_dy:
    :return:
    """

    # return 2 * R * math.sin(180 * ori_dy / (2 * math.pi * R))
    return 2 * R * math.sin(ori_dy / (2 * R))


def cal_distance(p1, p2):
    """

    :param p1: tuple (x1,y1,z1)
    :param p2: tuple (x2,y2,z2)
    :return:
    """
    x = p1[0] - p2[0]
    y = p1[1] - p2[1]
    z = p1[2] - p2[2]
    return math.sqrt(x * x + y * y + z * z)


def cal_real_z():
    pass


"""
调试工具，确定正面的地面纵坐标
"""


def get_PIN_line_value(x, y, z, iter=10, X_SUB=0.0125):
    """
    通过焊锡最高点，获取PIN针的面的深度坐标
    :param x:
    :param y:
    :param z:
    :param iter: 迭代次数
    :param X_SUB: 线扫间距
    :return: PIN针的面的中心坐标的深度坐标、横纵坐标
    """
    # 找到最高点的横坐标
    # HIGHTEST_POINT_X = x.min()  # pcd文件是min
    HIGHTEST_POINT_X = x.max()  # npy现场文件是max
    # print(HIGHTEST_POINT_X)
    Z_MEANS = []
    for i in range(iter):
        # index = x == HIGHTEST_POINT_X + i * X_SUB  # pcd文件是＋
        index = x == HIGHTEST_POINT_X - i * X_SUB  # npy文件是-
        Z_SET = z[index]
        Y_SET = y[index]
        X_SET = x[index]
        Z_MEANS.append(Z_SET.mean())
    # print("last {}".format(np.mean(Z_MEANS)))

    # Version1.0 不计算 先注释
    # x_center = ( X_SET.min() + X_SET.max() ) / 2
    # y_center = ( Y_SET.min() + Y_SET.max() ) / 2
    Z_MEANS = [x for x in Z_MEANS if not np.isnan(x)]
    # PIN_CENTER_POINT = (np.mean(Z_MEANS), x_center, y_center)
    PIN_CENTER_POINT = (np.mean(Z_MEANS), 0, 0)
    # print(PIN_CENTER_POINT)
    return PIN_CENTER_POINT


def get_center_point(x, y, z):
    # 先找到第一个PIN针，最高点 得到中心坐标为(x0,y0,z0)
    # XR = x0 + PIN针高度
    # YR = y0
    # ZR = z0 - 半径
    pass
